#include <cstdio>

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "epuckDriver/GetOdometry.h"

#include <unistd.h>

int main(int argc,char **argv){

    ros::init(argc,argv,"testServiceOdomAll");
    ros::NodeHandle n;
    ros::ServiceClient  cliGetOdom[6];
    
    epuckDriver::GetOdometry response[6];
        
    cliGetOdom[0] = n.serviceClient<epuckDriver::GetOdometry>("epuckGetOdom1");
    cliGetOdom[1] = n.serviceClient<epuckDriver::GetOdometry>("epuckGetOdom2");
    cliGetOdom[2] = n.serviceClient<epuckDriver::GetOdometry>("epuckGetOdom3");
    cliGetOdom[3] = n.serviceClient<epuckDriver::GetOdometry>("epuckGetOdom4");
    cliGetOdom[4] = n.serviceClient<epuckDriver::GetOdometry>("epuckGetOdom5");
    cliGetOdom[5] = n.serviceClient<epuckDriver::GetOdometry>("epuckGetOdom6");
    
    ros::Rate loopRate(1);
   // while(true){

        for(int j=0;j<6;j++){        
    
            if( j != 1)
                cliGetOdom[j].call(response[j]);       
   
                 
            printf("%d: x: %lf y: %lf theta: %lf\n",j+1,
response[j].response.pose2d.x,response[j].response.pose2d.y,response[j].response.pose2d.theta);
        }
        
        ros::spinOnce();
    
        loopRate.sleep();    
 //   }
    return 0;
}
